#include "ubt_state/ubt_state_util.h"
#include "ubt_state/ubt_state_task_action.h"
#include "ubt_state/node.h"
#include "ubt_interface/srv/task_action.hpp"

namespace ubt_state
{
struct ActionClsData
{
    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr _sub_action;
    rclcpp::Client<ubt_interface::srv::TaskAction>::SharedPtr _cli_action;
    CTaskAction *_pCurTask;
};
static ActionClsData g_clsData;

void Action_Status_CallbackWrap(const std_msgs::msg::String::SharedPtr msg)
{
    if (g_clsData._pCurTask != NULL)
    {
        g_clsData._pCurTask->Action_Status_Callback(msg);
    }
}

int CTaskAction::ClassInit()
{
    ActionClsData *pClsData = &g_clsData;
    pClsData->_sub_action = node->create_subscription<std_msgs::msg::String>("/task_actions", 10, std::bind(&Action_Status_CallbackWrap, std::placeholders::_1));
    pClsData->_cli_action = node->create_client<ubt_interface::srv::TaskAction>("/task_action_service");
    pClsData->_pCurTask = NULL;
    return 0;
}

void CTaskAction::ClassCleanup()
{
    ActionClsData *pClsData = &g_clsData;
    // pClsData->_sub_action->Shutdown();
    //pClsData->_cli_action
    pClsData->_pCurTask = NULL;
}

ITask *CTaskAction::CreateTask(ITaskCb *pTaskCb, std::string taskId, Action *pAction, int index, int reportIndex)
{
    CTaskAction *pTask = new CTaskAction(pTaskCb, taskId, pAction, index, reportIndex);
    return (ITask *)pTask;
}

CTaskAction::CTaskAction(ITaskCb *pTaskCb, std::string taskId, Action *pAction, int index, int reportIndex)
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s  _index=%d actionType=%s value=%s",
		taskId.c_str(),index,pAction->type.c_str(),pAction->value.c_str());
    _pTaskCb = pTaskCb;
    _taskId = taskId;
    _action = *pAction;
    _index = index;
    _reportIndex = reportIndex;
    _state = ubt_task_state_idle;
    _flag = 0;
    _stopReason = ubt_task_result_ok;
}

CTaskAction::~CTaskAction()
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s  _index=%d _state=%d actionType=%s ", 
		_taskId.c_str(),_index, _state, _action.type.c_str());
    if (g_clsData._pCurTask == this)
    {
        g_clsData._pCurTask = NULL;
    }
}


bool CTaskAction::Start()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state != ubt_task_state_idle)
    {
        RCLCPP_WARN(node->get_logger(),"fail bad state _state=%d", _state);
        return false;
    }
    _state = ubt_task_state_running;
    return true;
}

void CTaskAction::Pause()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state == ubt_task_state_running)
    {
        _state = ubt_task_state_pause;
    }
}

void CTaskAction::Continue()
{
    RCLCPP_INFO(node->get_logger(),"_state=%d", _state);
    if (_state == ubt_task_state_pause)
    {
        _state = ubt_task_state_running;
    }
}

void CTaskAction::Stop(int reason)
{
    RCLCPP_INFO(node->get_logger(),"_state=%d reason=%d", _state, reason);
    if (_state >= ubt_task_state_stopping)
    {
        return;
    }
    _stopReason = reason;
    _stopReasonDesc = UbtStateGetTaskResultDesc(_stopReason);
    if (reason == ubt_task_result_usr_cancel)
    {
        //cancel 需要等待action_done
        _state = ubt_task_state_stopping;
        return;
    }
    _SetState(ubt_task_state_closed, reason, "");
}

void CTaskAction::OnTimer()
{
    if ((_state == ubt_task_state_running) && (_flag == 0))
    {
        // ubt_task_action::task_action::Request req;
        auto request = std::make_shared<ubt_interface::srv::TaskAction::Request>();
        std::string reqStr;
        bool bRes = false;
        char buf[256];
        int maxLen = sizeof(buf) - 1;
        buf[maxLen] = 0;
        int len = snprintf(buf, maxLen, "[{\"action\":\"%s\", \"value\":\"%s\"}]", _action.type.c_str(), _action.value.c_str());
        if (len >= maxLen)
        {
            RCLCPP_WARN(node->get_logger(),"fail too long len=%d maxLen=%d", len, maxLen);
        }
        else
        {
            request->action = std::string(buf);
            // ubt_task_action::task_action::Response rsp;
            RCLCPP_INFO(node->get_logger(),"Action=%s",request->action);
            while (!g_clsData._cli_action->wait_for_service(1s)) { 
                RCLCPP_INFO(node->get_logger(),"/task_action_service service not available, waiting again...");
            }

            auto result = g_clsData._cli_action->async_send_request(request); 
            if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS)  
            {
                bRes = true;
                // RCLCPP_INFO(node->get_logger(),"请求正常处理,响应结果: is success: %d, msg: %s",result.get()->success, result.get()->message.c_str());
            }
            else
            {
                RCLCPP_WARN(node->get_logger(),"请求处理失败....");
                return;
            }
            // if (!bRes)
            // {
            //     RCLCPP_WARN(node->get_logger(),"fail call ");
            // }
        }
        if (bRes)
        {
            _flag |= 0x01;
            g_clsData._pCurTask = this;
        }
        else
        {
            _SetState(ubt_task_state_closed, ubt_task_result_action_abort, "");
        }
    }
}

void CTaskAction::OnMsg(int msgType, void *pvMsg)
{
}

ubt_task_type CTaskAction::GetType()
{
    return ubt_task_type_action;
}

ubt_task_state CTaskAction::GetState()
{
    return _state;
}

ubt_task_stage CTaskAction::GetStage()
{
    return ubt_task_stage_action;
}

std::string CTaskAction::GetActionType()
{
    return _action.type;
}

void CTaskAction::GetIndex(int& index, int& reportIndex)
{
    index = _index;
    reportIndex = _reportIndex;
}

void CTaskAction::GetStopInfo(int& stopReason, std::string& stopReasonDesc)
{
    stopReason = _stopReason;
    stopReasonDesc = _stopReasonDesc;
}

void CTaskAction::GetLogs(std::vector<TaskLogItem>& logs)
{

}


void CTaskAction::Action_Status_Callback(const std_msgs::msg::String::SharedPtr msg)
{
    
    std::string msgData = msg->data;

    ubt_state::ulogger->info("[CTaskAction] Action_Status_Callback : _state={}, msgData={}", _state, msgData.c_str());

    RCLCPP_INFO(node->get_logger(),"_state=%d msgData=%s", _state, msgData.c_str());
    do
    {
        if (msgData == "ACTION_FAIL")
        {
            _SetState(ubt_task_state_closed, ubt_task_result_action_fail, "action fail");
            break;
        }
        else if (msgData == "ACTION_CANCEL")
        {
            _SetState(ubt_task_state_closed, ubt_task_result_action_cancel, "action cancel");
        }
        else if (msgData != "ACTION_DONE")
        {
            break;
        }
        
        if ((_state == ubt_task_state_idle) || (_state == ubt_task_state_closed))
        {
            break;
        }
        
        _SetState(ubt_task_state_closed, ubt_task_result_ok, "");
    }
    while (false);
    
}

void CTaskAction::_SetState(ubt_task_state newState, int reason, std::string reasonStr)
{
    RCLCPP_INFO(node->get_logger(),"taskId=%s newState=%d oldState=%d reason=%d reasonStr=%s",
             _taskId.c_str(), newState, _state, reason, reasonStr.c_str());

    ubt_state::ulogger->info("[CTaskAction] _SetState : taskId={} newState={} oldState={} reason={} reasonStr={}",
             _taskId.c_str(), newState, _state, reason, reasonStr.c_str());

    if (newState == _state)
    {
        return;
    }
    int oldState = _state;
    _state = newState;

    if (_state == ubt_task_state_running)
    {
    }
    if (_state == ubt_task_state_closed)
    {
        _stopReason = reason;
        _stopReasonDesc = reasonStr; 
        
        if (g_clsData._pCurTask == this)
        {
            g_clsData._pCurTask = NULL;
        }
#ifdef UBT_LIFT_SUPPORT
        if ((_action.type == UBT_ACTION_FALL) || (_action.type == UBT_ACTION_RISE))
        {
            StateStatus *pStateStatus = _pTaskCb->GetStateStatus();
            if (pStateStatus->liftState != lift_state_up)
            {
                pStateStatus->isUnderShelve = true;
                pStateStatus->shelvePose = pStateStatus->curPose;
            }
            else
            {
                pStateStatus->isUnderShelve = false;
            }
        }
#endif
    }
     
    _pTaskCb->NotifyTaskStateAndStage(GetState(), GetStage(), reason, reasonStr);
}


}


